package com.sensors;

import android.util.Log;

/**
 * Created by tjpu on 2016/10/12.
 */
public class UWB {
    static String thisLogTag = "UWB";

    public double receivePeriod = 0.1;
    static double INIT_NUM = 10000.0;
    public static double CONVERT_TO_METRIC = 0.01;
    static int NUM_FOR_READY = 4;
    static double IMPOSSIBLE_DIST_MIN = 0.1; //UWB的返回值不可能的值

    int refreshCnt = 0;
    public boolean isReady = false;

    long lastTime = System.currentTimeMillis();
    public double[] origin = createArray(4, INIT_NUM);
    public double[] originLast = createArray(4, INIT_NUM);
    double[] metricUnit = createArray(origin.length, INIT_NUM);
    double[] metricUnitLast = createArray(origin.length, INIT_NUM);


    public int notReceive = 0;
    public boolean isNotReceive = false;

    public void putOrigin(int id, int distance) {
        if (id > origin.length - 1) {
            double[] x = createArray(id, INIT_NUM);
            double[] y = createArray(id, INIT_NUM);
            double[] z = createArray(id, INIT_NUM);
            System.arraycopy(origin, 0, x, 0, origin.length);
            System.arraycopy(metricUnit, 0, y, 0, metricUnit.length);
            System.arraycopy(metricUnitLast,0, y, 0, metricUnitLast.length);
            origin = x;
            metricUnit = y;
            metricUnitLast = z;
        }

        origin[id] = distance;

        metricUnit[id] = convert(origin[id]);
        judge(id);
        filter(id);

        notReceive = 0;
        isNotReceive = false;
        receivePeriod = (System.currentTimeMillis() - lastTime) /  1000.0;
        lastTime = System.currentTimeMillis();
        Log.i(thisLogTag,"receive period: "+receivePeriod);
    }

    public double[] getMetric() {
        double[] returnData = new double[metricUnit.length];
        System.arraycopy(metricUnit,0,returnData,0,returnData.length);
        return returnData;
    }

    double convert(double data) {
        return data * CONVERT_TO_METRIC;
    }

    void judge(int id) {
        if (metricUnit[id] < IMPOSSIBLE_DIST_MIN) {
            Log.e(thisLogTag,"receive wrong data");  //测距不成功，值为0
            metricUnit[id] = metricUnitLast[id];
        }

        refreshCnt ++;
        if (refreshCnt >= NUM_FOR_READY) {
            isReady = true;
        }
    }

    void filter(int id) {
        if ((metricUnit[id] - metricUnitLast[id] > 0.15) && (metricUnitLast[id] < INIT_NUM - 10)) {
            Log.i(thisLogTag,"filter"+" "+(metricUnit[id] - metricUnitLast[id])+"  "+metricUnitLast[id]);
//            metricUnit[id] = metricUnitLast[id] + 0.1 * metricUnit[id]/Math.abs(metricUnit[id]);
            metricUnit[id] = convert(originLast[id]);
        }
    }

    public void checkReceive() {
        notReceive++;
        if (notReceive > 3 && !isNotReceive) {
            isNotReceive = true;
            Log.e(thisLogTag,"can't receive data");
        }
    }

    double[] createArray(int length,double initNum) {
        double[] array = new double[length];
        for (int i=0; i < length; i++) {
            array[i] = initNum;
        }

        return array;
    }

    public void reset() {
        System.arraycopy(metricUnit,0,metricUnitLast,0,metricUnitLast.length);
        System.arraycopy(origin,0,originLast,0,originLast.length);
        for (int i=0; i<origin.length; i++) {
            origin[i] = INIT_NUM;
            metricUnit[i] = INIT_NUM;
        }
        refreshCnt = 0;
        isReady = false;
    }
}
